﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Doubao
{
    public class DoubaoDroneGeoConvert
    {
        public static void LMain()
        {
            var converter = new DJICoordinateConverter();
            // 若有相机安装偏移，需设置（例如：相机在机体前方0.1m，右0.05m）
            // converter.CameraMountOffset = new Point3D(0.1, 0.05, 0);

            // 输入参数（用户提供）
            // 无人机姿态（Yaw相对北，Pitch抬头为正，Roll右倾为正）
            double droneRoll = 15;       // 右倾15°
            double droneYaw = 88.90;     // 北偏东88.9°
            double dronePitch = -18.4;   // 低头18.4°

            // 无人机位置
            var droneLla = new LatLonAlt(
                lat: 36.003380555555559,
                lon: 100.20005,
                alt: 3109.71
            );
            double relativeAlt = 69.662; // 距地面高度

            // 云台姿态（Yaw相对北，Pitch向下为负，Roll右倾为正）
            double gimbalRoll = 180;     // 翻转180°（需确认实际方向）
            double gimbalYaw = -89.30;   // 相对北顺时针89.3°（即北偏西0.7°）
            double gimbalPitch = -90;    // 垂直向下

            // 相机参数
            int imgWidth = 640, imgHeight = 512;
            double focal = 24;       // 24mm
            double pixelSizeUm = 12; // 12μm

            // 计算点
            PixelPoint centerPixel = new PixelPoint(imgWidth / 2.0, imgHeight / 2.0); // 图像中心
            PixelPoint targetPixel = new PixelPoint((169 + 192) / 2.0, (253 + 296) / 2.0); // 目标中心

            // 转换
            var centerGeo = converter.PixelToGeo(
                centerPixel, imgWidth, imgHeight, focal, pixelSizeUm,
                gimbalYaw, gimbalPitch, gimbalRoll,
                droneYaw, dronePitch, droneRoll,
                droneLla, relativeAlt
            );
            var targetGeo = converter.PixelToGeo(
                targetPixel, imgWidth, imgHeight, focal, pixelSizeUm,
                gimbalYaw, gimbalPitch, gimbalRoll,
                droneYaw, dronePitch, droneRoll,
                droneLla, relativeAlt
            );

            // 输出
            Console.WriteLine("最终优化结果：");
            Console.WriteLine($"图像中心点：{centerGeo}");
            Console.WriteLine($"目标中心点：{targetGeo}");

            // 验证：垂直向下时，中心点应接近无人机正下方
            Console.WriteLine("\n验证：垂直向下时，中心点纬度应接近无人机纬度：" +
                $"{Math.Abs(centerGeo.Lat - droneLla.Lat) < 0.0001}");
        }
    }

    // 三维点结构体（支持运算符重载，便于调试）
    public struct Point3D
    {
        public double X, Y, Z;
        public Point3D(double x, double y, double z) => (X, Y, Z) = (x, y, z);
        public override string ToString() => $"X:{X:F2}, Y:{Y:F2}, Z:{Z:F2}";
    }

    // 像素点结构体
    public struct PixelPoint
    {
        public double U, V;
        public PixelPoint(double u, double v) => (U, V) = (u, v);
    }

    // 经纬度高度结构体
    public struct LatLonAlt
    {
        public double Lat, Lon, Alt;
        public LatLonAlt(double lat, double lon, double alt) => (Lat, Lon, Alt) = (lat, lon, alt);
        public override string ToString() => $"Lat:{Lat:F8}, Lon:{Lon:F8}, Alt:{Alt:F2}";
    }

    public static class GeoConstants
    {
        // WGS84官方参数
        public const double A = 6378137.0;          // 长半轴(m)
        public const double B = 6356752.314245;     // 短半轴(m)
        public const double E2 = 0.00669437999014;  // 第一偏心率平方
        public const double E_PRIME2 = 0.00673949674227; // 第二偏心率平方
    }

    public static class MatrixUtil
    {
        // 角度转弧度
        public static double ToRad(double deg) => deg * Math.PI / 180.0;
        // 弧度转角度
        public static double ToDeg(double rad) => rad * 180.0 / Math.PI;

        // 3x3矩阵乘法
        public static double[,] Multiply(double[,] a, double[,] b)
        {
            var res = new double[3, 3];
            for (int i = 0; i < 3; i++)
                for (int j = 0; j < 3; j++)
                    res[i, j] = a[i, 0] * b[0, j] + a[i, 1] * b[1, j] + a[i, 2] * b[2, j];
            return res;
        }

        // 矩阵与向量乘法
        public static Point3D Multiply(double[,] m, Point3D v)
        {
            return new Point3D(
                m[0, 0] * v.X + m[0, 1] * v.Y + m[0, 2] * v.Z,
                m[1, 0] * v.X + m[1, 1] * v.Y + m[1, 2] * v.Z,
                m[2, 0] * v.X + m[2, 1] * v.Y + m[2, 2] * v.Z
            );
        }
    }

    public class DJICoordinateConverter
    {
        // 相机在机体上的安装偏移（单位：m，需实际校准，默认无偏移）
        public Point3D CameraMountOffset { get; set; } = new Point3D(0, 0, 0);

        /// <summary>
        /// 像素→图像坐标系(mm)（关键修正：Y轴反转）
        /// 图像坐标系定义：原点为主点，X右，Y上（与像素V轴向下相反）
        /// </summary>
        public Point3D PixelToImage(PixelPoint pixel, double u0, double v0, double pixelSizeMm)
        {
            return new Point3D(
                (pixel.U - u0) * pixelSizeMm,  // X：右为正
                -(pixel.V - v0) * pixelSizeMm,   // Y：上为正（反转像素V轴）
                0 // Z：图像坐标系Z恒为0（补充此参数）
            );
        }

        /// <summary>
        /// 图像→相机坐标系(m)
        /// 相机坐标系：原点为光心，X右，Y上，Z前（光轴方向）
        /// </summary>
        public Point3D ImageToCamera(Point3D imagePt, double focalLengthMm, double zc)
        {
            double fM = focalLengthMm / 1000.0; // 焦距(m)
            return new Point3D(
                imagePt.X / 1000.0 * (zc / fM),  // Xc = x * zc / f
                imagePt.Y / 1000.0 * (zc / fM),  // Yc = y * zc / f
                zc                               // Zc = zc（沿光轴向前）
            );
        }

        /// <summary>
        /// 相机→机体坐标系（考虑云台绝对姿态+安装偏移）
        /// 云台姿态定义（大疆官方）：
        /// - Yaw：相对地理北（顺时针为正）
        /// - Pitch：向下为负，向上为正
        /// - Roll：右倾为正
        /// 旋转顺序：Yaw→Pitch→Roll（外旋，绕固定轴）
        /// </summary>
        public Point3D CameraToBody(Point3D cameraPt, double gimbalYaw, double gimbalPitch, double gimbalRoll)
        {
            // 转换为弧度
            double yaw = MatrixUtil.ToRad(gimbalYaw);    // 外旋用正角度
            double pitch = MatrixUtil.ToRad(gimbalPitch);
            double roll = MatrixUtil.ToRad(gimbalRoll);

            // Yaw旋转矩阵（绕Z轴，顺时针为正）
            var rYaw = new double[,] {
            { Math.Cos(yaw), Math.Sin(yaw), 0 },  // 顺时针旋转：sin符号为正
            { -Math.Sin(yaw), Math.Cos(yaw), 0 },
            { 0, 0, 1 }
        };

            // Pitch旋转矩阵（绕Y轴，向下为负，外旋）
            var rPitch = new double[,] {
            { Math.Cos(pitch), 0, -Math.Sin(pitch) },
            { 0, 1, 0 },
            { Math.Sin(pitch), 0, Math.Cos(pitch) }
        };

            // Roll旋转矩阵（绕X轴，右倾为正，外旋）
            var rRoll = new double[,] {
            { 1, 0, 0 },
            { 0, Math.Cos(roll), Math.Sin(roll) },
            { 0, -Math.Sin(roll), Math.Cos(roll) }
        };

            // 外旋顺序：Yaw→Pitch→Roll（矩阵乘法顺序相反）
            var rTotal = MatrixUtil.Multiply(rRoll, MatrixUtil.Multiply(rPitch, rYaw));
            Point3D rotated = MatrixUtil.Multiply(rTotal, cameraPt);

            // 叠加相机安装偏移（相机原点相对机体原点的位置）
            return new Point3D(
                rotated.X + CameraMountOffset.X,
                rotated.Y + CameraMountOffset.Y,
                rotated.Z + CameraMountOffset.Z
            );
        }

        /// <summary>
        /// 机体→地理坐标系（NED：北东地）
        /// 机体坐标系：X前，Y右，Z下
        /// 无人机姿态：Yaw（相对北顺时针）、Pitch（抬头为正）、Roll（右倾为正）
        /// 旋转顺序：Yaw→Pitch→Roll（外旋）
        /// </summary>
        public Point3D BodyToNed(Point3D bodyPt, double droneYaw, double dronePitch, double droneRoll)
        {
            double yaw = MatrixUtil.ToRad(droneYaw);
            double pitch = MatrixUtil.ToRad(dronePitch);
            double roll = MatrixUtil.ToRad(droneRoll);

            // Yaw旋转矩阵（绕Z轴，顺时针为正）
            var rYaw = new double[,] {
            { Math.Cos(yaw), Math.Sin(yaw), 0 },
            { -Math.Sin(yaw), Math.Cos(yaw), 0 },
            { 0, 0, 1 }
        };

            // Pitch旋转矩阵（绕Y轴，抬头为正）
            var rPitch = new double[,] {
            { Math.Cos(pitch), 0, -Math.Sin(pitch) },
            { 0, 1, 0 },
            { Math.Sin(pitch), 0, Math.Cos(pitch) }
        };

            // Roll旋转矩阵（绕X轴，右倾为正）
            var rRoll = new double[,] {
            { 1, 0, 0 },
            { 0, Math.Cos(roll), Math.Sin(roll) },
            { 0, -Math.Sin(roll), Math.Cos(roll) }
        };

            // 外旋总矩阵
            var rTotal = MatrixUtil.Multiply(rRoll, MatrixUtil.Multiply(rPitch, rYaw));
            return MatrixUtil.Multiply(rTotal, bodyPt);
        }

        /// <summary>
        /// NED→ECEF坐标系
        /// </summary>
        public Point3D NedToEcef(Point3D nedPt, LatLonAlt droneLla)
        {
            double lat = MatrixUtil.ToRad(droneLla.Lat);
            double lon = MatrixUtil.ToRad(droneLla.Lon);

            // NED到ECEF的旋转矩阵（北→X, 东→Y, 地→Z）
            var rot = new double[,] {
            { -Math.Sin(lat)*Math.Cos(lon), -Math.Sin(lon), -Math.Cos(lat)*Math.Cos(lon) },
            { -Math.Sin(lat)*Math.Sin(lon), Math.Cos(lon), -Math.Cos(lat)*Math.Sin(lon) },
            { Math.Cos(lat), 0, -Math.Sin(lat) }
        };

            // 旋转NED坐标
            Point3D rotated = MatrixUtil.Multiply(rot, nedPt);

            // 无人机ECEF坐标
            Point3D droneEcef = LlaToEcef(droneLla);

            // 目标ECEF = 无人机ECEF + 旋转后的NED偏移
            return new Point3D(
                droneEcef.X + rotated.X,
                droneEcef.Y + rotated.Y,
                droneEcef.Z + rotated.Z
            );
        }

        /// <summary>
        /// 经纬度→ECEF
        /// </summary>
        public Point3D LlaToEcef(LatLonAlt lla)
        {
            double lat = MatrixUtil.ToRad(lla.Lat);
            double lon = MatrixUtil.ToRad(lla.Lon);
            double h = lla.Alt;

            double N = GeoConstants.A / Math.Sqrt(1 - GeoConstants.E2 * Math.Pow(Math.Sin(lat), 2));
            return new Point3D(
                (N + h) * Math.Cos(lat) * Math.Cos(lon),
                (N + h) * Math.Cos(lat) * Math.Sin(lon),
                (N * (1 - GeoConstants.E2) + h) * Math.Sin(lat)
            );
        }

        /// <summary>
        /// ECEF→经纬度（高精度迭代）
        /// </summary>
        public LatLonAlt EcefToLla(Point3D ecef)
        {
            double x = ecef.X;
            double y = ecef.Y;
            double z = ecef.Z;

            double p = Math.Sqrt(x * x + y * y);
            double lon = Math.Atan2(y, x);

            // 初始纬度估计
            double lat = Math.Atan2(z, p * (1 - GeoConstants.E2));
            double N = GeoConstants.A / Math.Sqrt(1 - GeoConstants.E2 * Math.Pow(Math.Sin(lat), 2));
            double h = p / Math.Cos(lat) - N;

            // 迭代优化（10次确保精度）
            for (int i = 0; i < 10; i++)
            {
                lat = Math.Atan2(z + GeoConstants.E2 * N * Math.Sin(lat), p);
                N = GeoConstants.A / Math.Sqrt(1 - GeoConstants.E2 * Math.Pow(Math.Sin(lat), 2));
                h = p / Math.Cos(lat) - N;
            }

            return new LatLonAlt(
                MatrixUtil.ToDeg(lat),
                MatrixUtil.ToDeg(lon),
                h
            );
        }

        /// <summary>
        /// 完整转换流程
        /// </summary>
        public LatLonAlt PixelToGeo(
            PixelPoint pixel,
            // 相机参数
            double imageWidth, double imageHeight,
            double focalLengthMm, double pixelSizeUm,
            // 云台姿态（绝对Yaw：相对北）
            double gimbalYaw, double gimbalPitch, double gimbalRoll,
            // 无人机姿态（Yaw相对北）
            double droneYaw, double dronePitch, double droneRoll,
            // 无人机位置与高度
            LatLonAlt droneLla, double relativeAltitude
        )
        {
            // 1. 主点坐标（图像中心，假设无畸变）
            double u0 = imageWidth / 2.0;
            double v0 = imageHeight / 2.0;
            double pixelSizeMm = pixelSizeUm / 1000.0;

            // 2. 计算zc（目标到相机的沿光轴距离）
            double zc = relativeAltitude / Math.Cos(MatrixUtil.ToRad(Math.Abs(gimbalPitch)));
            if (Math.Abs(gimbalPitch + 90) < 1) zc = relativeAltitude; // 垂直向下

            // 3. 像素→图像
            Point3D imagePt = PixelToImage(pixel, u0, v0, pixelSizeMm);

            // 4. 图像→相机
            Point3D cameraPt = ImageToCamera(imagePt, focalLengthMm, zc);

            // 5. 相机→机体（云台Yaw为绝对角度，无需叠加无人机Yaw）
            Point3D bodyPt = CameraToBody(cameraPt, gimbalYaw, gimbalPitch, gimbalRoll);

            // 6. 机体→NED（无人机姿态转换）
            Point3D nedPt = BodyToNed(bodyPt, droneYaw, dronePitch, droneRoll);

            // 7. NED→ECEF
            Point3D ecefPt = NedToEcef(nedPt, droneLla);

            // 8. ECEF→经纬度
            return EcefToLla(ecefPt);
        }
    }
}
